clc
clear
currentDir = pwd; % 获取当前目录路径
parentDir = fileparts(currentDir); % 获取当前目录的上一级目录路径
% grandParentDir = fileparts(parentDir); % 获取当前目录的上两级目录路径
% greatGrandParentDir = fileparts(grandParentDir); % 获取当前目录的上三级目录路径

%指定需要查找的文件夹
subdir = 'DATA';
%指定需要查找的文件名
filename = '413168000_org.mat';
%构造需要查找的文件的完整路径
full_path = fullfile(parentDir, subdir, filename);
current_data = load(full_path);%当前船舶的ais数据
current_data = current_data.AIS;
% current_data (current_data.Destination ~='CJK',:)=[];
ship_AIS =current_data(:,{'Lon','Lat'});

% 创建图形，但不显示在屏幕上
% figure(1);
% DrawENC(ship_AIS, 'Original AIS Trajectory');
plot(ship_AIS.Lon(265:290),ship_AIS.Lat(265:290),'k-','LineWidth',1);
% 显示网格
grid on;
hold on
%绘制异常点
plot(ship_AIS.Lon(270:271),ship_AIS.Lat(270:271),'ro');%p2\3
%绘制相邻点
plot(ship_AIS.Lon([269 272]),ship_AIS.Lat([269 272]),'r*');%p1\4

% legend(['MMSI:' +MMSI],'轨迹突变点');
legend('船舶轨迹','轨迹急转点','相邻轨迹点');


% xlabel('Lon(°)');
% ylabel('Lat(°)');
xlabel('经度(°)');
ylabel('纬度(°)');

fontSize=8;
fontName = 'Times New Roman'; % 字体名称
% 创建 text
text(122.131853532278 ,29.9487573469212,'P1','FontSize', fontSize, 'FontName', fontName);%
text(122.131763868617, 29.9487904622018,'P4','FontSize', fontSize, 'FontName', fontName);%
text(122.131487234668, 29.9487378799717,'P2','FontSize', fontSize, 'FontName', fontName);%
text(122.131797548683, 29.94877610093553,'P3','FontSize', fontSize, 'FontName', fontName);


window_size_bf = 2;%前窗口大小
window_size_af = 4;

data_error =ship_AIS(270:271,:);
lonlat_bf_window =[ship_AIS.Lon(267:268),ship_AIS.Lat(267:268)];
lonlat_af_window =[ship_AIS.Lon(273:276),ship_AIS.Lat(273:276)];

%绘制圆形边界
lonlat_mean_bf=[mean(lonlat_bf_window(:,1)),mean(lonlat_bf_window(:,2))];%前窗口经纬度的均值
lonlat_mean_af=[mean(lonlat_af_window(:,1)),mean(lonlat_af_window(:,2))];%后窗口经纬度的均值

% 计算圆心（中点）
center = (lonlat_mean_bf + lonlat_mean_af) / 2;
% 计算圆的半径（直径的一半）
radius = norm(lonlat_mean_af - lonlat_mean_bf) / 2;
% 计算检查点到圆心的距离
distances2center = sqrt((data_error.Lon - center(1)).^2 + (data_error.Lat - center(2)).^2);%求解异常点集中每个点到圆心的距离
% 判断每个点是否在圆内或圆上
in_circle = distances2center <= radius;
% 创建一个圆的点集，使用linspace和cos()/sin()函数
theta = linspace(0, 2*pi, 100); % 参数方程的参数值
lon = center(1) + radius * cos(theta); % 圆的x坐标
lat = center(2) + radius * sin(theta); % 圆的y坐标
plot(lon, lat, 'k--','LineWidth',1); % 使用蓝色线条绘制圆
plot(center(1),center(2),'kp');
text(122.131819663805, 29.9487307477499,'O', 'Interpreter', 'tex','FontSize', fontSize, 'FontName', fontName);%
%绘制窗口数据和窗口边界

plot(lonlat_bf_window(:,1),lonlat_bf_window(:,2),'bs');%前窗口数据
plot(lonlat_af_window(:,1),lonlat_af_window(:,2),'bd');%
% 使用 TeX 标记在文本中添加下标
textStr_pb1 = 'P_{b1} ';%and P ^{sup}';
textStr_pb2 = 'P_{b1-w_{bf}+1} ';
% 绘制文本
% text(x, y, textStr, 'Interpreter', 'tex');
text(122.131886946746 ,29.9487217213085,textStr_pb1, 'Interpreter', 'tex','FontSize', fontSize, 'FontName', fontName);%
text(122.131956080815, 29.9487256537064,textStr_pb2, 'Interpreter', 'tex','FontSize', fontSize, 'FontName', fontName);% );%

textStr_pa1 = 'P_{a1} ';%and P ^{sup}';
textStr_pa2 = 'P_{a1+w_{af}-1} ';

text(122.13172768445, 29.9488159586891,textStr_pa1, 'Interpreter', 'tex','FontSize', fontSize, 'FontName', fontName);% );%
text(122.13162847139, 29.9488114104384,textStr_pa2, 'Interpreter', 'tex','FontSize', fontSize, 'FontName', fontName);%


window_len = 0.00002;%窗口短边长度，绘制窗口边界专用
window_boundary_bf_lon=[lonlat_bf_window(1,1)
    lonlat_bf_window(1,1)
    lonlat_bf_window(end,1)
    lonlat_bf_window(end,1)
    lonlat_bf_window(1,1)];%前窗口边界坐标
window_boundary_bf_lat=[lonlat_bf_window(1,2)+window_len
    lonlat_bf_window(1,2)-window_len
    lonlat_bf_window(end,2)-window_len
    lonlat_bf_window(end,2)+window_len
    lonlat_bf_window(1,2)+window_len];
plot(window_boundary_bf_lon,window_boundary_bf_lat,'m--');%前边界数据

window_boundary_af_lon=[lonlat_af_window(1,1)
    lonlat_af_window(1,1)
    lonlat_af_window(end,1)
    lonlat_af_window(end,1)
    lonlat_af_window(1,1)];
window_boundary_af_lat=[lonlat_af_window(1,2)+window_len
    lonlat_af_window(1,2)-window_len
    lonlat_af_window(end,2)-window_len
    lonlat_af_window(end,2)+window_len
    lonlat_af_window(1,2)+window_len];
plot(window_boundary_af_lon,window_boundary_af_lat,'m--');




legend('船舶轨迹','轨迹急转点','相邻轨迹点','圆形边界','圆心O','前窗口数据集','后窗口数据集','窗口边界');


%插值测试
data_spline =current_data([267:269,271:276],{'time','Lon','Lat'});
data_spline_time = data_spline.time-data_spline.time(1);
data_spline_lon=data_spline.Lon;
data_spline_lat = data_spline.Lat;

time_interp=table2array(current_data(270,'time')-data_spline.time(1));

% 对经度进行三次样条插值
Slon = spline(data_spline_time, data_spline_lon, time_interp);
pp_lon=spline(data_spline_time, data_spline_lon);

% 对纬度进行三次样条插值
Slat = spline(data_spline_time, data_spline_lat, time_interp);
data_spline_inter = [Slon Slat];

spline_data = [data_spline_time table2array(data_spline(:,{'Lon','Lat'}));time_interp Slon Slat];
spline_data=sortrows(spline_data,1);


figure
plot(spline_data(:,2),spline_data(:,3),'b*-');
hold on 
plot(spline_data(4,2),spline_data(4,3),'ro');


